import math
import numpy as np


class Geolation:
    def __init__(self, time: int, latitude: float, longitude: float):
        self.time = time
        self.latitude = latitude                          # x轴, 纬度
        self.longitude = longitude


def print_geo(geolation: Geolation):
    print('['+str(geolation.time) + ',', str(geolation.latitude) + ',', str(geolation.longitude) + ']')


def print_geolist(tra: list[Geolation]):
    for geo in tra:
        print_geo(geo)


# 定义速度函数， 其返回值为两个坐标点间的距离（不是速度）
class Speed:
    def get_speed(self) -> float:
        return 0.0


class KeepSpeed(Speed):
    def __init__(self, speed):
        self.speed = speed

    def get_speed(self) -> float:
        return self.speed


def solve(a: float, b: float, c: float) -> float:          # 解一元二次方程
    return (-b + math.sqrt(b * b - 4 * a * c))/2*a


class Trajectory:
    @staticmethod
    def get_trajectory(vector: Speed = KeepSpeed(3)) -> list[Geolation]:
        dt = 2000          # 每个坐标点时间差
        now = Geolation(0, np.random.normal(0, 0.02), np.random.normal(-0.6, 0.005))
        ret = [now]
        length = 0.0

        # 标准化下进行生成
        while length < 1200:
            ds = vector.get_speed()
            if -0.4 <= now.latitude <= 0.4 and now.longitude <= 0.0:        # 西侧直道
                theory_point = Geolation(now.time, now.latitude, -0.6)
                new_point = Geolation(theory_point.time + dt, np.random.normal(theory_point.latitude - ds / 75.0, 0.005), np.random.normal(-0.6, 0.005))
                ret.append(new_point)
                now = new_point
                length += ds
            elif now.latitude < -0.4:  # 南侧弯道
                zeta = math.atan(now.longitude/(now.latitude + 0.4)) + math.pi
                target = np.random.normal(zeta - ((ds * math.pi) / 45), 0.04)
                new_point = Geolation(now.time + dt, 0.6*math.cos(target) - 0.4, 0.6*math.sin(target))
                ret.append(new_point)
                now = new_point
                length += ds
            elif -0.4 <= now.latitude <= 0.4 and now.longitude > 0.0:
                theory_point = Geolation(now.time, now.latitude, 0.6)
                new_point = Geolation(theory_point.time + dt, np.random.normal(theory_point.latitude + ds / 75.0, 0.005), np.random.normal(0.6, 0.005))
                ret.append(new_point)
                now = new_point
                length += ds
            elif now.latitude > 0.4:
                zeta = math.atan(now.longitude/(now.latitude - 0.4))
                target = np.random.normal(zeta - ((ds * math.pi) / 45), 0.04)
                new_point = Geolation(now.time + dt, 0.6*math.cos(target) + 0.4, 0.6*math.sin(target))
                ret.append(new_point)
                now = new_point
                length += ds
            else:
                break

        # 反标准化
        std_numerator = 0.000748225519274115       # 标准化的分子
        dx = 36.12432223
        dy = 120.4846737

        for point in ret:
            point.latitude = point.latitude * std_numerator + dx
            point.longitude = point.longitude * std_numerator + dy

        return ret


